#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import device_manager.arm_manager 
import device_manager.gripper_manager 
import rospy
import numpy as np
import tf.transformations 
from  geometry_msgs.msg import Pose

class demov2():
    def __init__(self):
        self.gm   = device_manager.gripper_manager.GripperManager()
        self.arm  = device_manager.arm_manager.ArmManager()

        self.position = np.zeros((3,),dtype=float)
        self.orientation = np.zeros((4,),dtype=float)

        # 设置允许的最大速度和加速度，默认均为0.3
        self.arm.set_max_acceleration_and_velocity()

    def __pose_callback(self, msgpose):
       
        print(msgpose)
        # 姿态读取测试，关节角控制测试
        # pose = self.arm.get_current_pose()
        # print(pose)
        self.arm.set_joint_values(j0=0.1)
        
        self.position[0] = msgpose.position.x
        self.position[1] = msgpose.position.y
        self.position[2] = msgpose.position.z
        # 姿态控制测试，p：xyz，o：xyzw
        self.arm.back_to_zero()

        orientation = [0.70692,0,0,0.70692]
        self.arm.set_target_pose(self.position)

        # 二指爪控制测试
        # self.gm.activate()
        # self.gm.open()
        # rospy.loginfo('gripper opened.')
        # rospy.sleep(2)
        # self.gm.close()
        # rospy.loginfo('gripper closed.')
        # rospy.sleep(2)
        # self.gm.open(speed=100)
        # rospy.loginfo('gripper opened.')
        # rospy.sleep(2)
        # self.gm.close(speed=50)
        # rospy.loginfo('gripper closed.')
        # rospy.sleep(2)
        # self.gm.open(force=50)
        # rospy.loginfo('gripper opened.')
        # rospy.sleep(2)
        # self.gm.close(force=20)
        # rospy.loginfo('gripper closed.')
        # rospy.sleep(5)
    
    def run(self):
        pose = self.arm.get_current_pose()
        print pose
        n_q = tf.transformations.quaternion_from_euler(0,1.5707,0)
        print n_q
        self.arm.set_target_pose(orientation=n_q)
        rospy.Subscriber("grasp_detection/predict_pose", Pose, self.__pose_callback, queue_size=1)
        rospy.spin()

if __name__ == '__main__':
    # 初始化ros
    rospy.init_node('DEVICE_MANAGER', anonymous=False)
    app = demov2()
    app.run()


 



   

